#include "robot.h"
#include <string.h>

#define MOTOR_STOP_SIGN 0X00
static const uint8_t step_seq[8] = {0x01, 0x03, 0x02, 0x06, 0x04, 0x0C, 0x08, 0x09};

static motor_info_t xdata g_motor_list[MOTOR_MAX_NUM] = {0};
static motor_sign_write_func write_motor_sign = NULL;

uint8_t freq_div_num[MOTOR_MAX_NUM] = {0};

motor_info_t *get_motor_list(void)
{
	return g_motor_list;
}

void robot_init(motor_sign_write_func func)
{
	uint8_t i;
	for (i = 0; i < MOTOR_MAX_NUM; i++)
	{
		g_motor_list[i].motor_num = i;
		g_motor_list[i].seq_index = 0;
		g_motor_list[i].sign_sta = 0;
		g_motor_list[i].cur_step_num = 0;
		g_motor_list[i].target_step_num = 0;
		g_motor_list[i].freq_div = 0;
		g_motor_list[i].work_mode = MOTOR_STOP_MODE;
		g_motor_list[i].idle_mode = MOTOR_STOP_MODE;
	}
	
	write_motor_sign = func;
}

void robot_goto_target(motor_name_e motor_num, uint32_t target)
{
	if (g_motor_list[motor_num].cur_step_num < target)
	{
		g_motor_list[motor_num].dir = 1;
	}
	else
	{
		g_motor_list[motor_num].dir = 0;
	}
	g_motor_list[motor_num].target_step_num = target;
	g_motor_list[motor_num].work_mode = MOTOR_AUTO_MODE;
}

void robot_run_tick(void)
{
	uint8_t out_sign = 0;
	uint8_t i;
	
	for (i = 0; i < MOTOR_MAX_NUM; i++)
	{
		if (freq_div_num[i] < g_motor_list[i].freq_div) {
			freq_div_num[i]++;
			continue;
		} else {
			freq_div_num[i] = 0;
		}

		switch (g_motor_list[i].work_mode)
		{
			case MOTOR_STOP_MODE:
				out_sign = MOTOR_STOP_SIGN;
			break;
			case MOTOR_MANUAL_MODE:
				if (g_motor_list[i].dir)
				{
					g_motor_list[i].seq_index++;
					g_motor_list[i].cur_step_num++;
				}
				else
				{
					g_motor_list[i].seq_index--;
					g_motor_list[i].cur_step_num--;
				}
				out_sign = step_seq[g_motor_list[i].seq_index % 8];
			break;
			case MOTOR_AUTO_MODE:
				if (g_motor_list[i].cur_step_num == g_motor_list[i].target_step_num)
				{
					g_motor_list[i].work_mode = g_motor_list[i].idle_mode;
					break;
				}
				if (g_motor_list[i].dir)
				{
					if (g_motor_list[i].cur_step_num < g_motor_list[i].target_step_num)
					{
						g_motor_list[i].seq_index++;
						g_motor_list[i].cur_step_num++;
					}
					else
					{
					g_motor_list[i].work_mode = g_motor_list[i].idle_mode;
						break;
					}
				}
				else
				{
					if (g_motor_list[i].cur_step_num > g_motor_list[i].target_step_num)
					{
						g_motor_list[i].seq_index--;
						g_motor_list[i].cur_step_num--;
					}
					else
					{
					g_motor_list[i].work_mode = g_motor_list[i].idle_mode;
						break;
					}
				}
				out_sign = step_seq[g_motor_list[i].seq_index % 8];
			break;
			case MOTOR_KEEP_MODE:
				if (g_motor_list[i].sign_sta == MOTOR_STOP_SIGN) {
					out_sign = step_seq[g_motor_list[i].seq_index % 8];
				}
				else
				{
					out_sign = MOTOR_STOP_SIGN;
				}
			break;
			default : break;
		}
		g_motor_list[i].sign_sta = out_sign;
		write_motor_sign(g_motor_list[i].motor_num, g_motor_list[i].sign_sta);
	}
}


